﻿
// 2018/6/20: 首个记录
// 2018/10/23: FromGeneralSample支持不带轨迹的样本
// 2018/11/29: 增加Protocols静态属性
// 2019/3/22: 时间戳赋值
// 2019/3/29: 添加注释
// 2019/8/8: 未来路径注释修正

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ASEva;

namespace VehicleSampleV3
{
    enum GearPosition
    {
        Unknown = 0, // 未知
        Park = 1, // P档
        Reverse = 2, // R档
        Neutral = 3, // N档
        Drive = 4, // D档
    };
    enum TurnState
    {
        Unknown = 0, // 未知
        None = 1, // 未打灯
        TurnLeft = 2, // 左转向灯
        TurnRight = 3, // 右转向灯
        Emergency = 4, // 双闪
    };
    enum Horn
    {
        H_Unknown = 0, // 未知
        H_None = 1, // 不响
        H_Active = 2, // 响
    };

    enum HeadLight
    {
        HL_Unknown = 0, // 未知
        HL_None = 1, // 未激活
        HL_Near = 2, // 近光
        HL_Far = 3, // 远光
    };

    enum Wiper
    {
        W_Unknown = 0, // 未知
        W_None = 1, // 未激活
        W_Single = 2, // 单次
        W_Slow1 = 3, // 慢 1
        W_Slow2 = 4, // 慢 2
        W_Slow3 = 5, // 慢 3
        W_Slow4 = 6, // 慢 4
        W_Slow5 = 7, // 慢 5
        W_Medium = 8, // 中档
        W_Fast = 9, // 快档
    };


    class VehicleSample : Sample
    {
        public double Speed { get; set; } // 本车车速 kph
        public double KilometerAge { get; set; } // 总里程 km
        public double? YawRate { get; set; } // 横摆角速度 deg/s
        public double? Curvature { get; set; } // 行驶曲率 deg/s 左转为正 未来路径: y=(Curvature/2)*(x^2)+(Curvature)*(FrontOverhang+WheelBase)*x
        public double? AccelX { get; set; } // 本车纵向加速度 m/s2
        public double? AccelY { get; set; } // 本车横向加速度 m/s2
        public PointF[] Trajectory { get; set; } // 历史路径

        public double VehicleWidth { get; set; } // 车宽 m
        public double VehicleLength { get; set; } // 车长 m
        public double SteerAngleRatio { get; set; } // 传动比
        public double WheelBase { get; set; } // 轴距 m
        public double RearTread { get; set; } // 轮距 m
        public double FrontOverhang { get; set; } //前悬 m

        public double? SteerAngle { get; set; } // 方向盘转角 deg
        public double? Brake { get; set; } // 刹车位置 %
        public double? Throttle { get; set; } // 油门位置 %
        public GearPosition Gear { get; set; } // 档位
        public TurnState Turn { get; set; } // 转向灯
        public Horn Horn { get; set; } // 喇叭
        public HeadLight HeadLight { get; set; } // 大灯
        public Wiper Wiper { get; set; } // 雨刮

        public double? FLSpeed { get; set; } // 左前轮轮速 kph
        public double? FRSpeed { get; set; } // 右前轮轮速 kph
        public double? RLSpeed { get; set; } // 左后轮轮速 kph
        public double? RRSpeed { get; set; } // 右后轮轮速 kph

        public double? EngineSpeed { get; set; } // 引擎转速 rpm
        public double? EngineTorque { get; set; } // 引擎扭矩 Nm

        public override bool FromGeneralSample(GeneralSample sample)
        {
            if (sample.Protocol == "vehicle-sample-v3")
            {
                if (sample.Values.Count < 27) return false;

                int trajectorySize = (int)sample.Values[0].number;
                int sizeWithoutTraj = 27;
                int sizeWithTraj = 27 + 2 * trajectorySize;

                if (sample.Values.Count != sizeWithoutTraj && sample.Values.Count != sizeWithTraj) return false;

                Speed = sample.Values[1].IsNumber() ? sample.Values[1].number : 0;
                YawRate = sample.Values[2].ToDouble();
                Curvature = sample.Values[3].ToDouble();
                AccelX = sample.Values[4].ToDouble();
                AccelY = sample.Values[5].ToDouble();
                VehicleWidth = sample.Values[6].IsNumber() ? sample.Values[6].number : 1.9;
                VehicleLength = sample.Values[7].IsNumber() ? sample.Values[7].number : 4.6;
                SteerAngleRatio = sample.Values[8].IsNumber() ? sample.Values[8].number : 15;
                WheelBase = sample.Values[9].IsNumber() ? sample.Values[9].number : 2.8;
                RearTread = sample.Values[10].IsNumber() ? sample.Values[10].number : 1.6;
                FrontOverhang = sample.Values[11].IsNumber() ? sample.Values[11].number : 0.9;
                SteerAngle = sample.Values[12].ToDouble();
                Brake = sample.Values[13].ToDouble();
                Throttle = sample.Values[14].ToDouble();
                Gear = sample.Values[15].IsNumber() ? (GearPosition)(int)sample.Values[15].number : GearPosition.Unknown;
                Turn = sample.Values[16].IsNumber() ? (TurnState)(int)sample.Values[16].number : TurnState.Unknown;
                FLSpeed = sample.Values[17].ToDouble();
                FRSpeed = sample.Values[18].ToDouble();
                RLSpeed = sample.Values[19].ToDouble();
                RRSpeed = sample.Values[20].ToDouble();
                EngineSpeed = sample.Values[21].ToDouble();
                EngineTorque = sample.Values[22].ToDouble();
                Horn = sample.Values[23].IsNumber() ? (Horn)(int)sample.Values[23].number : Horn.H_Unknown;
                HeadLight = sample.Values[24].IsNumber() ? (HeadLight)(int)sample.Values[24].number : HeadLight.HL_Unknown;
                Wiper = sample.Values[25].IsNumber() ? (Wiper)(int)sample.Values[25].number : Wiper.W_Unknown;
                KilometerAge = sample.Values[26].IsNumber() ? sample.Values[26].number : 0;

                if (sample.Values.Count == sizeWithTraj)
                {
                    Trajectory = new PointF[trajectorySize];
                    for (int i = 0; i < trajectorySize; i++)
                    {
                        Trajectory[i] = new PointF((float)sample.Values[27 + 2 * i].number, (float)sample.Values[27 + 2 * i + 1].number);
                    }
                }
                else
                {
                    Trajectory = new PointF[0];
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }

            return false;
        }
        public override string GetGeneralSampleProtocol()
        {
            return "vehicle-sample-v3";
        }
        public static string[] Protocols
        {
            get
            {
                return new String[] { "vehicle-sample-v3" };
            }
        }
        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }
        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as VehicleSample;
            var s2 = input.s2 as VehicleSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new VehicleSample();

            output.Speed = s1.Speed * w1 + s2.Speed * w2;
            output.KilometerAge = s1.KilometerAge * w1 + s2.KilometerAge * w2;
            output.YawRate = s1.YawRate * w1 + s2.YawRate * w2;
            output.Curvature = s1.Curvature * w1 + s2.Curvature * w2;
            output.AccelX = s1.AccelX * w1 + s2.AccelX * w2;
            output.AccelY = s1.AccelY * w1 + s2.AccelY * w2;
            output.VehicleWidth = s1.VehicleWidth * w1 + s2.VehicleWidth * w2;
            output.VehicleLength = s1.VehicleLength * w1 + s2.VehicleLength * w2;
            output.SteerAngleRatio = s1.SteerAngleRatio * w1 + s2.SteerAngleRatio * w2;
            output.WheelBase = s1.WheelBase * w1 + s2.WheelBase * w2;
            output.RearTread = s1.RearTread * w1 + s2.RearTread * w2;
            output.FrontOverhang = s1.FrontOverhang * w1 + s2.FrontOverhang * w2;
            output.SteerAngle = s1.SteerAngle * w1 + s2.SteerAngle * w2;
            output.Brake = s1.Brake * w1 + s2.Brake * w2;
            output.Throttle = s1.Throttle * w1 + s2.Throttle * w2;
            output.Gear = w1 > w2 ? s1.Gear : s2.Gear;
            output.Turn = w1 > w2 ? s1.Turn : s2.Turn;
            output.Horn = w1 > w2 ? s1.Horn : s2.Horn;
            output.HeadLight = w1 > w2 ? s1.HeadLight : s2.HeadLight;
            output.Wiper = w1 > w2 ? s1.Wiper : s2.Wiper;
            output.FLSpeed = s1.FLSpeed * w1 + s2.FLSpeed * w2;
            output.FRSpeed = s1.FRSpeed * w1 + s2.FRSpeed * w2;
            output.RLSpeed = s1.RLSpeed * w1 + s2.RLSpeed * w2;
            output.RRSpeed = s1.RRSpeed * w1 + s2.RRSpeed * w2;
            output.EngineSpeed = s1.EngineSpeed * w1 + s2.EngineSpeed * w2;
            output.EngineTorque = s1.EngineTorque * w1 + s2.EngineTorque * w2;
            output.Trajectory = w1 > w2 ? s1.Trajectory : s2.Trajectory;

            return output;
        }
        public override bool SupportInterpolation()
        {
            return true;
        }
    }
}
